#include "rp_motor/gripper.h"

// constructor
GripperNode::GripperNode() : nh_("~") {
	timestamp_ = ros::Time::now();
	gripper_fstate = gripper_cstate = GRIPPER_OPEN;
	init();
	motor_sub_ = nh_.subscribe(Topic::MOTOR_GRIPPER, 1, &GripperNode::gripperCallback, this);
	status_pub_ = nh_.advertise<comm::gripper>(Topic::GRIPPER_STATUS, 10);	// publish status
}

// destructor
GripperNode::~GripperNode() {
	nh_.shutdown();
}

/**
 * Init gripper node.
 */
void GripperNode::init()
{
	// initialize motor
	ROS_INFO("Initialize gripper motor.");

	// read parameters (like the dpr2 robot)
	std::string motor_port_name, motor_config_name;

	ROS_ASSERT(nh_.getParam("motor_port", motor_port_name));
	//ROS_ASSERT(nh_.getParam("motor_config", motor_config_name));

	// load motor configuration
	//CXMLConfiguration motor_config_xml;
	//ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	CDxlConfig *config = new CDxlConfig();
	//config.readConfig(motor_config_xml.root().section("motor"));
	motor_ = new C3mxlROS(motor_port_name.c_str());
	motor_->setConfig(config->setID(GRIPPER_PORT));

	// Initialize motor
	ros::Rate init_rate(1);
	while (ros::ok() && motor_->init() != DXL_SUCCESS)
	{
		ROS_WARN_ONCE("Couldn't initialize gripper motor, will continue trying every second");
		init_rate.sleep();
	}

	motor_->set3MxlMode(PWM_MODE);

	// close motor
	motor_->setPWM(GRIPPER_PWM_CLOSED);

	// wait a sec
	ros::Time sec = ros::Time::now() + ros::Duration(1);
	while(sec > ros::Time::now());

	// open motor
	motor_->setPWM(GRIPPER_PWM_OPEN);

	delete config;
}

/**
 * Handle incoming messages for the gripper.
 * @param	msg	expected state for the gripper.
 */
void GripperNode::gripperCallback(const comm::gripper::ConstPtr &msg)
{
	gripper_fstate = msg->state;
}
/**
 * Drive motor.
 */
void GripperNode::spin()
{
	ros::Rate loop_rate(20);
	while(ros::ok())
	{
		switch(gripper_fstate)
		{
		case GRIPPER_CLOSED:
			motor_->setPWM(GRIPPER_PWM_CLOSED);
			break;
		case GRIPPER_OPEN:
			if(gripper_cstate != GRIPPER_OPEN)	// send it once
			{
				timestamp_ = ros::Time::now() + ros::Duration(1);
				motor_->setPWM(GRIPPER_PWM_OPEN);
			}
			break;
		}

		gripper_cstate = gripper_fstate;
		// Status update after 1 second
		comm::gripper msg;
		if(ros::Time::now() > timestamp_)
			msg.state = gripper_cstate;
		else
			msg.state = !gripper_cstate;
		status_pub_.publish(msg);

		ros::spinOnce();
		loop_rate.sleep();
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "gripper_motor");
	GripperNode gripper;
	gripper.spin();
	return 0;
}
